# coding:utf-8

import math
from xml.dom import minidom


def set_attributes(dom, key_values):
    for key in key_values:
        dom.setAttribute(key, key_values[key])


def create_road_element(document, key_values):
    road = document.createElement('road')
    set_attributes(road, key_values)
    return road


def create_element(document, name, key_values):
    element = document.createElement(name)
    set_attributes(element, key_values)
    return element


def create_opendrive_test(filename):
    xml = minidom.Document()
    root = xml.createElement('OpenDRIVE')
    xml.appendChild(root)
    header = xml.createElement('header')
    set_attributes(header, {'revMajor': '1', 'revMinor': '1',
                            'name': 'Deep Motion Demo File',
                            'version': '0.0.1'})
    root.appendChild(header)
    for i in range(10):
        road = create_road_element(xml, {'name': 'test' + str(i), 'length': '121', 'id': str(i), 'junction': '-1'})
        root.appendChild(road)
        link = create_element(xml, 'link', {})
        predecessor = create_element(xml, 'predecessor',
                                     {'elementType': 'road', 'elementId': '5', 'contactPoint': 'start'})
        link.appendChild(predecessor)
        successor = create_element(xml, 'successor',
                                   {'elementType': 'road', 'elementId': '1', 'contactPoint': 'end'})
        link.appendChild(successor)
        road.appendChild(link)
        planView = create_element(xml, 'planView', {})
        fake_coords = [[0, 0, 6, 1], [10, 10, 5.7], [10, 0, 5.5], [20, 5, 5.5]]
        s = 0
        elevationProfile = create_element(xml, 'elevationProfile', {})
        for coord_i in range(1, len(fake_coords)):
            start, end = fake_coords[coord_i - 1], fake_coords[coord_i]
            length = math.sqrt((start[0] - end[0]) ** 2 + (start[1] - end[1]) ** 2)
            hdg = math.atan2(end[1] - start[1], end[0] - start[0]) * 180 / math.pi
            geometry = create_element(xml, 'geometry',
                                      {'s': str(s), 'x': str(start[0]), 'y': str(start[1]), 'hdg': str(hdg),
                                       'length': str(length)})
            line = create_element(xml, 'line', {})

            geometry.appendChild(line)
            planView.appendChild(geometry)
            elevation_per_distance = (end[2] - start[2]) / length
            elevation = create_element(xml, 'elevation',
                                       {'s': str(s), 'a': str(start[2]), 'b': str(elevation_per_distance), 'c': '0',
                                        'd': '0'})
            elevationProfile.appendChild(elevation)
            s += length
        road.appendChild(planView)
        road.appendChild(elevationProfile)
        fake_lanes={'left':[],}
        lanes = create_element(xml,'lanes',{})

    f = open(filename, 'w')
    f.write(xml.toxml())
    f.close()


if __name__ == '__main__':
    create_opendrive_test('test.xml')
